#include "../include/Minion.h"
#include "../include/Global.h"

#include "SDL/SDL.h"
#include "SDL/SDL_opengl.h"
//Takes starting Pos of the Minon as a parameter.
Minion::Minion(Position P)
{
    DLine = false;
    Pos = P;
    PrevPos = Pos;
    health = 50.0;
    maxHealth = 50;
    SpeedLocation = 2;
    speedVect.push_back(.025);
    speedVect.push_back(.05);
    speedVect.push_back(.1);
    speedVect.push_back(.2);
    speedVect.push_back(.25);
    speed = speedVect[SpeedLocation];
    colorR = 0.8;
    colorG = 0;
    colorB = 0;
    slow = false;
}
//Need to learn how to make a deconstructor.
Minion::~Minion()
{
    //dtor
}
//returns the current Position of the Minion
Position const Minion::getPos()
{
    return Pos;
}

void Minion::setPath(std::vector<Position> newpath){
    MyBestPath = newpath;
    goal = newpath[newpath.size() - 1];
}


// Sets bestPath as the current shortest path using A*
//Test path and Bestpath will use a different algorithm.
///TODO: Check path from both directions. Also If I store the last Position that you were at
/// And the penalty, I can speed up hero Pathfinding by a lot. Interesting code though. THINK about this more.

std::vector<Position> Minion::bestPath(const std::vector<Position>& towerPos, const Position destination)
{
    MyBestPath = testPath(towerPos, destination);
    return MyBestPath;
}
Position Minion :: GetGoal()
{
    return goal;
}
//returns if tempPos in the vector of positions
bool Minion::HaveVisited(std::vector<Position>& visited, Position tempPos)
{
    return !(find(visited.begin(),visited.end(),tempPos) == visited.end());
}
//Just choose what to add to the array.
bool Minion::ModifySpeed(double multiplier)
{
//    bool CanComply = (SpeedLocation + multiplier >= 0 && SpeedLocation + multiplier < speedVect.size());
//    if(CanComply){
//        speed = speedVect[SpeedLocation*multiplier];
//    }
//    return CanComply;
    speed = multiplier;
    return 1;
}
double Minion::changeHealth(double newHealth)
{
    health = newHealth;
    return health;
}
//Toggles Draw line
void Minion::DrawLine()
{
   Uint8 *keyState = SDL_GetKeyState(NULL);

    //If escape is pressed and left alt is pressed
    if( keyState[ SDLK_0])
    {
        DLine = !DLine;
    }


}
double const Minion::getHealth()
{
    return health;
}
void Minion::ModifyColor(float r, float g, float b)
{
    colorR = r;
    colorG = g;
    colorB = b;
}
//Draws the minion to screen
void Minion::Draw()
{

    //Mainly just Draws the minions path to help debug the pathfinding algorithm.
    DrawLine();
    if(true){
    glPushMatrix();
    glDisable(GL_LIGHTING);
    glTranslatef(0,0,.3);


    glBegin(GL_LINE_STRIP);
    glColor3f(1, 1, 0);

    for (int k = 0; k < MyBestPath.size(); k++)
    glVertex3f(MyBestPath[k].Xpos, MyBestPath[k].Ypos, 0.0f);
    glEnd();
    glEnable(GL_LIGHTING);
    glPopMatrix();
    }


    glPushMatrix();
    glTranslatef(Pos.Xpos, Pos.Ypos, GROUND + 0.0001); // Sets the Pos to the minion. Nothing should need to be moved to
    //The minions Pos anymore. It is already there IE (0,0,0) is the minion.
    glLoadName(nMINION);
    glPushName(index);
    glBegin(GL_QUADS);
    //Minion PlaceHolders Draws it a []
    glColor3f(colorR, colorG, colorB);
    glVertex3f(-0.25f, -0.25f, 0.0f);
    glVertex3f(0.25f, -0.25f, 0.0f);
    glVertex3f(0.25f, 0.25f, 0.0f);
    glVertex3f(-0.25f, 0.25f, 0.0f);
    glEnd();
    glPopName();
    glLoadName(nGROUND);



    //Draws Minion Health
    glPushMatrix();

    glTranslatef(0,0, 1.5);
    glBegin(GL_QUADS);
    glColor3f(1-(health/maxHealth), health/maxHealth, 0.0f);
    glVertex3f(0, -0.05f, 0);
    glVertex3f(0.7f * (health/maxHealth), -0.05f, 0);
    glVertex3f(0.7f * (health/maxHealth), 0.05f, 0);
    glVertex3f(0, 0.05f, 0);
    glColor3f(.8,.8,.8);
    glVertex3f(0.f, -0.05f, 0.0f);
    glVertex3f(0.7f, -0.05f, 0.0f);
    glVertex3f(0.7f, 0.05f, 0.0f);
    glVertex3f(0.f, 0.05f, 0.0f);
    glEnd();
    glPopMatrix();


    glPopMatrix();
}
//Need to get Direction and move the speed in that Direction.
//You have to hit directly on the pos or It will never stop.
void Minion::Update()
{

    if (MyBestPath.size() >= 2)
    {
        if(AtNextPoint(PrevPos, Pos, MyBestPath[1])) // I am at the next Pos
        {
            PrevPos = MyBestPath[1];
            Pos = MyBestPath[1];
            MyBestPath.erase(MyBestPath.begin());
        }
        float Xmovement = MyBestPath[1].Xpos - PrevPos.Xpos;
        float Ymovement = MyBestPath[1].Ypos - PrevPos.Ypos;
        Pos.Xpos += (double)(Xmovement*speed);
        Pos.Ypos += (double)(Ymovement*speed);
    }
    if(slow)
        CheckSlowDuration();
}

void Minion::SetGoal(Position g){
goal = g;
}

void Minion :: SetIndex(int i)
{
    index = i;
}
bool Minion::AtNextPoint(Position Prev, Position Spot, Position Goal){ // Will only work for moving in 4 directions.
    bool answer = false;
    if(     Prev.Xpos - Goal.Xpos < 0 && (Prev.Ypos - Goal.Ypos == 0)){ //Going Right
        answer = Spot.Xpos > Goal.Xpos || Spot == Goal;
    }
    else if(Prev.Xpos - Goal.Xpos > 0 && (Prev.Ypos - Goal.Ypos == 0)){ //Going Left
        answer = Spot.Xpos < Goal.Xpos || Spot == Goal;
    }
    else if(Prev.Ypos - Goal.Ypos > 0 && (Prev.Xpos - Goal.Xpos == 0)){ //Going Down
        answer = Spot.Ypos < Goal.Ypos || Spot == Goal;
    }
    else if(Prev.Ypos - Goal.Ypos < 0 && (Prev.Xpos - Goal.Xpos == 0)){ //Going Up
        answer =  Spot.Ypos > Goal.Ypos || Spot == Goal;
    }
    else  if(Prev.Xpos - Goal.Xpos < 0 && Prev.Ypos - Goal.Ypos < 0){ //Going Right Up
        answer = (Spot.Xpos > Goal.Xpos && Spot.Ypos > Goal.Ypos) || Spot == Goal;
    }
    else if(Prev.Xpos - Goal.Xpos > 0 && (Prev.Ypos - Goal.Ypos < 0)){ //Going Left Up
        answer = (Spot.Xpos < Goal.Xpos && Spot.Ypos > Goal.Ypos) || Spot == Goal;
    }
    else if(Prev.Ypos - Goal.Ypos > 0 && (Prev.Xpos - Goal.Xpos < 0)){ //Going Down Right
        answer = (Spot.Ypos < Goal.Ypos && Spot.Xpos > Goal.Xpos ) || Spot == Goal;
    }
    else if(Prev.Ypos - Goal.Ypos > 0 && (Prev.Xpos - Goal.Xpos > 0)){ //Going Down Left
                answer =  (Spot.Ypos < Goal.Ypos && Spot.Xpos < Goal.Xpos) || Spot == Goal;
    }


    return answer;
}


std::vector<Position> Minion::testPath(const std::vector<Position>& towerPos, const Position destination)
{
    // Pos = PrevPos; // Need to fix this it makes it teleport.
    std::vector<Position> solution;

    goal = destination;
    int Lpen, Rpen, Upen, Dpen; //to store the penalties for moving in a certian direction.
    bool found = false;
    std::vector<Position> visited = towerPos;

    std::priority_queue<Position, std::vector < std::vector < Position > >, CompareDist> pathsQ;
    std::vector<Position> myPos;
    if(MyBestPath.size() <=2)
    {
        myPos.push_back(PrevPos); //starting point
    }
    if(MyBestPath.size() >2)
    {
        myPos.push_back(MyBestPath[0]);
        myPos.push_back(MyBestPath[1]);
    }
    myPos.push_back( Position(0,0) ); //This hold the total distance.
    pathsQ.push(myPos);

//Now we can start the PathFinding.
    while(!pathsQ.empty()&& !found)
    {
    //    std::priority_queue<Position, std::vector < std::vector < Position > >, CompareStartDist> temp = pathsQ;
        std::vector<Position> tempPath = pathsQ.top();
        pathsQ.pop();
        Position tempPos = tempPath[tempPath.size()-2]; // The last one is the Dist need to go one back
        visited.push_back(tempPos);
//        while(!temp.empty()){
//        std::cout << temp.top().size() << std::endl;
//        temp.pop();
//
//
//        }
//       std::cout << tempPos.Xpos <<" , " << tempPos.Ypos << std::endl;
//       std::cout << goal.Xpos <<" , " << goal.Ypos << std::endl;


    SetPenalty(tempPos, goal, Lpen, Rpen,  Dpen, Upen);



        if(tempPos == goal)  //found it;
        {
            found = true;
            tempPath[tempPath.size() - 1] = tempPos;
            solution = tempPath;
        }
        else{ // I have not found the answer
        tempPos.Xpos -= 1;
        if(!HaveVisited(visited,tempPos)) //Check left
        {
        //std::cout << " Checking Left" << std::endl;
            Position Distance = tempPath[tempPath.size() -1]; // Stores distance
//            std::cout << Distance.Xpos << std::endl;
            std::vector<Position> leftPath = tempPath;
            leftPath[leftPath.size() -1] = tempPos; // Sets the final spot to TemPpos
            Distance.Xpos += Lpen; // Penalty Distance
            Distance.Ypos += 1; // Total Grid Distance
//            std::cout << Distance.Xpos << std::endl;
            visited.push_back(tempPos);
            leftPath.push_back(Distance);
            pathsQ.push(leftPath);
        }
        tempPos.Xpos += 2;
        if(!HaveVisited(visited,tempPos)) //Check right
        {
            Position Distance = tempPath[tempPath.size() -1]; // Stores distance
            std::vector<Position> rightPath = tempPath;
            rightPath[rightPath.size() -1] = tempPos; // Sets the final spot to TemPpos
            Distance.Xpos += Rpen; // Penalty Distance
            Distance.Ypos += 1; // Total Grid Distance
            visited.push_back(tempPos);
            rightPath.push_back(Distance);
            pathsQ.push(rightPath);
        }
        tempPos.Xpos -= 1; //center back up
        tempPos.Ypos += 1;
        if(!HaveVisited(visited,tempPos)) //Check up
        {
            Position Distance = tempPath[tempPath.size() -1]; // Stores distance
            std::vector<Position> upPath = tempPath;
            upPath[upPath.size() -1] = tempPos; // Sets the final spot to TemPpos
            Distance.Xpos += Upen; // Penalty Distance
            Distance.Ypos += 1; // Total Grid Distance
            visited.push_back(tempPos);
            upPath.push_back(Distance);
            pathsQ.push(upPath);
        }
        tempPos.Ypos -= 2;
        if(!HaveVisited(visited,tempPos)) //Check down
        {
            Position Distance = tempPath[tempPath.size() -1]; // Stores distance
            std::vector<Position> downPath = tempPath;
            downPath[downPath.size() -1] = tempPos; // Sets the final spot to TemPpos
            Distance.Xpos += Dpen; // Penalty Distance
            Distance.Ypos += 1; // Total Grid Distance
            visited.push_back(tempPos);
            downPath.push_back(Distance);
            pathsQ.push(downPath);
        }
        }
        //I need to check to see if Goal is less than position at the start. Or maybe I should be updating
        // It real time.

    }
    return solution;
}
void Minion:: SlowStatus(float amount, double duration)
{
    tempspeed = speed;
    statusDuration = duration;
    statusTimer.start();
    ModifyColor(0,0,0.8);
    ModifySpeed(amount);
    slow = true;
}
void Minion :: CheckSlowDuration()
{
    if(statusTimer.elapsedTime() > statusDuration)
    {
        ModifySpeed(0.1);
        ModifyColor(0.8,0,0);
        slow = false;
    }
}
std::vector<Position> const Minion:: getBestPath()
{
    return MyBestPath;
}
double const Minion::GetSpeed(){
return speed;
}


//Changes the values of The Penaltys that were defined in the method (used in BestPath and TestPath)
void Minion::SetPenalty(const Position tempPos, const Position goal, int& Lpen, int& Rpen, int& Dpen, int& Upen)
{
    int best = 1; //Towards Goal
    int worse = 5; // Away from goal
    int neutral = 2; // Neutral

    if(tempPos.Xpos - goal.Xpos < 0 &&
        fabs(tempPos.Xpos - goal.Xpos) >= fabs(tempPos.Ypos - goal.Ypos) ){
    Lpen = worse;
    Rpen = best;
    Upen = neutral;
    Dpen = neutral;
    }
    //I moved Left more than I have moved up or down
    if(tempPos.Xpos - goal.Xpos > 0 &&
        fabs(tempPos.Xpos -goal.Xpos) >= fabs(tempPos.Ypos - goal.Ypos) ){
    Lpen = best;
    Rpen = worse;
    Upen = neutral;
    Dpen = neutral;
    }
    //I moved UP more or the same that I have moved left or right
    if(tempPos.Ypos - goal.Ypos  < 0 &&
       fabs(tempPos.Xpos -goal.Xpos) < fabs(tempPos.Ypos - goal.Ypos) ){
    Lpen = neutral;
    Rpen = neutral;
    Upen = best;
    Dpen = worse;
    }
    //I moved Down more or the same that I have moved left or right
     if(tempPos.Ypos - goal.Ypos  > 0 &&
       fabs(tempPos.Xpos -goal.Xpos) < fabs(tempPos.Ypos - goal.Ypos) ){
    Lpen = neutral;
    Rpen = neutral;
    Upen = worse;
    Dpen = best;
    }

}
